#include <stdlib.h>
#include <inttypes.h>
#include <arpa/inet.h>
#include "pms5005.h"

packet_t *dc_motor_position(uint8_t channel1, uint16_t position1, 
			uint8_t channel2, uint16_t position2, uint16_t time)
{
	packet_t *packet = NULL;
	uint8_t len = 6, *data = NULL;

	if ( 0 != time ) {
		len += 3;
	}

	data = (uint8_t *) malloc (len);
	data[0] = channel1;
	data[1] = (uint8_t) position1;
	data[2] = (uint8_t) (position1>>8);

	data[3] = channel2;
	data[4] = (uint8_t) position2;
	data[5] = (uint8_t) (position2>>8);

	if ( 0 != time ) {
		data[6] = 0x1c;
		data[7] = (uint8_t) time;
		data[8] = (uint8_t) (time>>8);
	}

	packet_new(&packet, 0x01, 0x00, MOT_POS, len, data);
	free(data);

	return packet;
}

packet_t *dc_motor_pwm(uint8_t channel, uint16_t pwm, uint16_t time)
{
	packet_t *packet = NULL;
	uint8_t *data = NULL, len = 0;

	if ( 0 == time ) {
		len = 3;
		data = (uint8_t *) malloc (len);
		data[0] = channel;
		data[1] = (uint8_t) pwm;
		data[2] = (uint8_t) (pwm>>8);
	} else {
		len = 6;
		data = (uint8_t *) malloc (len);
		data[0] = channel;
		data[1] = (uint8_t) pwm;
		data[2] = (uint8_t) (pwm>>8);
		data[3] = 0x1c;
		data[4] = (uint8_t) htons(time);
		data[5] = (uint8_t) (htons(time)>>8);
	}

	packet_new(&packet, 0x01, 0x00, MOT_PWM, len, data);
	free(data);

	return packet;
}

packet_t *dc_motor_velocity(uint8_t channel, uint16_t velocity, uint16_t time)
{
	packet_t *packet = NULL;
	uint8_t len, *data;

	if ( 0 == time ) {
		len = 3;
		data = (uint8_t *) malloc (len);
		data[0] = channel;
		data[1] = (uint8_t) velocity;
		data[2] = (uint8_t) (velocity>>8);
	} else {
		len = 6;
		data = (uint8_t *) malloc (len);
		data[0] = channel;
		data[1] = (uint8_t) velocity;
		data[2] = (uint8_t) (velocity>>8);
		data[3] = 0x1c;
		data[4] = (uint8_t) htons(time);
		data[5] = (uint8_t) (htons(time)>>8);
	}

	packet_new(&packet, 0x01, 0x00, MOT_VEL, len, data);
	free(data);

	return packet;
}


packet_t *servo_motor(uint8_t servo, uint16_t position, uint16_t time)
{
	packet_t *packet = NULL;
	uint8_t len, *data;

	if ( 0 == time ) {
		len = 3;
		data = (uint8_t *) malloc (len);
		data[0] = servo;
		data[1] = (uint8_t) position;
		data[2] = (uint8_t) (position>>8);
	} else {
		len = 6;
		data = (uint8_t *) malloc (len);
		data[0] = servo;
		data[1] = (uint8_t) position;
		data[2] = (uint8_t) (position>>8);
		data[3] = 0x1c;
		data[4] = (uint8_t) htons(time);
		data[5] = (uint8_t) (htons(time)>>8);
	}

	packet_new(&packet, 0x01, 0x00, SERVO, len, data);
	free(data);

	return packet;
}

packet_t *dc_motor_polarity(uint8_t channel, uint8_t polarity)
{
	packet_t *packet = NULL;
	uint8_t data[3];

	data[0] = 0x06;
	data[1] = channel;
	data[2] = polarity;

	packet_new(&packet, 0x01, 0x00, SETUP, 0x03, data);

	return packet;
}

packet_t *dc_motor_pid_position(uint8_t channel, uint8_t kp,
						uint8_t kd,
						uint8_t ki)
{
	packet_t *packet = NULL;
//	uint8_t *data = NULL;
//
//	packet_new(&packet, 0x01, 0x00, 0x07, 0x03, data);

	return packet;
}

packet_t *dc_motor_pid_velocity(uint8_t channel, uint8_t kp,
						uint8_t kd,
						uint8_t ki)
{
	packet_t *packet = NULL;

	return packet;
}

packet_t *dc_motor_sensor_usage(uint8_t channel, uint8_t usage)
{
	packet_t *packet = NULL;

	return packet;
}

packet_t *dc_motor_control_method(uint8_t channel, uint8_t method)
{
	packet_t *packet = NULL;
	uint8_t data[3];

	data[0] = 0x0e;
	data[1] = channel;
	data[2] = method;

	packet_new(&packet, 0x01, 0x00, SETUP, 0x03, data);

	return packet;
}

packet_t *dc_motor_state(uint8_t state)
{
	packet_t *packet = NULL;
	uint8_t data[1];

	data[0] = state;

	packet_new(&packet, 0x01, 0x00, MOT_STATUS, 0x01, data);

	return packet;
}

packet_t *get_motor_control_signal(uint8_t *n)
{
	packet_t *packet = NULL;
	uint8_t len = 0, *data = {NULL};

	if ( NULL == n ) {
		len = 0;
	} else {
		len = *n;
	}

	packet_new(&packet, 0x01, 0x00, 0x7b, len, data);

	return packet;
}

packet_t *get_sensor_feedback_signal(uint8_t *n)
{
	packet_t *packet = NULL;
	uint8_t len = 0, *data = NULL;

	if ( NULL == n ) {
		len = 0;
	} else {
		len = *n;
	}

	packet_new(&packet, RID_PMS5005, 0x00, SENSOR_FB, len, data);

	return packet;
}



packet_t *ping_pms5005()
{
	packet_t *packet = NULL;
	uint8_t len = 1, data[] = {1};

	packet_new(&packet, RID_PMS5005, 0x00, SYSTEM_COM, len, data);
	
	return packet;
}
